#include "serial_helper.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/byte_multi_array.hpp"
#include<rclcpp_components/register_node_macro.hpp>
#include <string>
#include <memory>
#include <thread>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/array.hpp>
#include <chrono>
#include <sstream>

using namespace std::chrono_literals;
using namespace std;
int fp;
unsigned char Msg[128] = { 0 };
const char* constc = nullptr;
SerialConfig sc;

using ByteData=std_msgs::msg::ByteMultiArray;

class TopicSuscribe01 : public rclcpp::Node
{
public:

    TopicSuscribe01(std::string nname) : Node(nname)
    {
        sc.baud = 115200;
        sc.port = "ttyS1";
        RCLCPP_INFO(this->get_logger(),"我是%s，订阅话题为：/uart_rec 发布话题为：uart_send .",nname.c_str());
        
        publisher_ = this->create_publisher<std_msgs::msg::ByteMultiArray>("uart_send", 10);    // CHANGE
        command_subscribe_ = this->create_subscription<std_msgs::msg::String>("uart_rec",11,std::bind(&TopicSuscribe01::command_callback,this,std::placeholders::_1));
      //  run_thread_ = std::thread(&SerialHelper::runThreadFunc, &serial);
      //  run_thread_.detach();
      // timer_ = this->create_wall_timer();
        timer_ = this ->create_wall_timer (10ms, std::bind(&TopicSuscribe01::timer_callback, this));

        serial.open("com1",sc);
        read_thread_ = std::thread(&SerialHelper::read, &serial);
        read_thread_.detach();
      // serial.setReadCallback(serial.readCallback);
    }
      
    void timer_callback()
    {
      //auto message = std_msgs::msg::String();
      int flag=0;
      std::vector<u8> uart_rec;
       std_msgs::msg::ByteMultiArray message_;
    //   char  sd[200];
      if(serial.get_rec_flag())
      {
            
        uart_rec = serial.get_rec_str();
        message_.data.resize(uart_rec.size());
         for(int i=0;i<uart_rec.size();i++)
         {
             message_.data[i]=uart_rec[i];
            
         }
        if(uart_rec[0]==0xaa)flag++;
        if(uart_rec[1]==0x0a)flag++;
        if(uart_rec[9]==0x0a)flag++;   ///遥控器命令号
        //ByteData _message{};
       // _message.data=uart_rec;
       
        //   auto message=ByteData(uart_rec);
        if(flag==3)
        {
        //RCLCPP_INFO(this->get_logger(), "Publishing: '%x |%x |%x '",message_.data[3],message_.data[4],message_.data[5]);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%x |%x |%x '",message_.data[3],message_.data[4],message_.data[5]);

        publisher_->publish(message_);
        }
      }
       
    }
private:
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_subscribe_;
    rclcpp::Publisher<std_msgs::msg::ByteMultiArray>::SharedPtr publisher_;         // CHANGE
    std::thread run_thread_;
    std::thread read_thread_;
    rclcpp::TimerBase::SharedPtr timer_;
    void command_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        double speed = 0.0f;
       //int length;
       
        RCLCPP_INFO(this->get_logger(),"收到的指令长度[%d] ",msg->data.length());
        
        constc = msg->data.c_str();
       // length=msg->data.length();
        
        serial.writeS(msg->data);
    }
    SerialHelper serial;
};


int main(int argc, char **argv)
{
    /* code */
    rclcpp::init(argc, argv);
    /*产生一个Wang2的节点*/
    // auto node = std::make_shared<rclcpp::Node>("pub_serial");
    auto node = std::make_shared<TopicSuscribe01>("node_uart");
    // 打印一句自我介绍
    // int fp;
    // Serial sp;
       // cout<<fp<<endl;

    RCLCPP_INFO(node->get_logger(), "大家好，我是topic_subscribe_01.");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
   // sp.ClosePort(fp);
    
    
    return 0;
}
